#include "bsp_motor.h"

#include "Mini58Series.h"
#include "string.h"
static void bsp_motor_pwm_Init(void);
static void bsp_motor_control_Init(void);

void bsp_motor_Init(void) {
  bsp_motor_pwm_Init();
  bsp_motor_control_Init();
}
void bsp_motor_control_Init(void) {
  GPIO_SetMode(MOT_EN_A_GPIO, MOT_EN_A_IO, GPIO_MODE_OUTPUT);
  GPIO_SetMode(MOT_DIR_A_GPIO, MOT_DIR_A_IO, GPIO_MODE_OUTPUT);

  GPIO_SetMode(MOT_EN_B_GPIO, MOT_EN_B_IO, GPIO_MODE_OUTPUT);
  GPIO_SetMode(MOT_DIR_B_GPIO, MOT_DIR_B_IO, GPIO_MODE_OUTPUT);
  SYS->P3_MFP = SYS_MFP_P36_GPIO;
  GPIO_SetMode(MOT_EN_C_GPIO, MOT_EN_C_IO, GPIO_MODE_OUTPUT);
  GPIO_SetMode(MOT_DIR_C_GPIO, MOT_DIR_C_IO, GPIO_MODE_OUTPUT);

  GPIO_SetMode(FAN_GPIO, FAN_IO, GPIO_MODE_OUTPUT);


  bsp_mot_set(MOT_GN_A, 0, 0);
  bsp_mot_set(MOT_GN_B, 0, 0);
  bsp_mot_set(MOT_GN_C, 0, 0);

  bsp_fan_set(0);
}
void bsp_motor_pwm_Init(void) {
  // PUL:  A-P04 CH5 B-P05 CH4 C-P06 CH1
  SYS_ResetModule(PWM0_RST);

  /* Enable IP clock */
  CLK_EnableModuleClock(PWMCH01_MODULE);
  CLK_EnableModuleClock(PWMCH45_MODULE);

  /* Select IP clock source */
  CLK_SetModuleClock(PWMCH01_MODULE, CLK_CLKSEL1_PWMCH01SEL_HCLK, 0);
  CLK_SetModuleClock(PWMCH45_MODULE, CLK_CLKSEL2_PWMCH45SEL_HCLK, 0);

  SYS->P0_MFP |=
      SYS_MFP_P06_PWM0_CH1 | SYS_MFP_P05_PWM0_CH4 | SYS_MFP_P04_PWM0_CH5;

  // PWM-Timer 0 enable and Auto-reload
  PWM->CTL = PWM_CTL_CNTEN1_Msk | PWM_CTL_CNTMODE1_Msk | PWM_CTL_CNTEN4_Msk |
             PWM_CTL_CNTMODE4_Msk | PWM_CTL_CNTEN5_Msk | PWM_CTL_CNTMODE5_Msk;
  //	PWM->CTL = PWM_CTL_CNTEN1_Msk | PWM_CTL_CNTMODE1_Msk;
  //	PWM->CTL = PWM_CTL_CNTEN4_Msk | PWM_CTL_CNTMODE4_Msk;

  PWM_SET_PRESCALER(PWM, 1, 2);
  PWM_SET_PRESCALER(PWM, 4, 2);
  PWM_SET_PRESCALER(PWM, 5, 2);

  PWM_SET_DIVIDER(PWM, 1, PWM_CLK_DIV_1);
  PWM_SET_DIVIDER(PWM, 4, PWM_CLK_DIV_1);
  PWM_SET_DIVIDER(PWM, 5, PWM_CLK_DIV_1);

  // Set the PWM aligned type
  PWM_SET_ALIGNED_TYPE(PWM, 1, PWM_EDGE_ALIGNED);
  PWM_SET_ALIGNED_TYPE(PWM, 4, PWM_EDGE_ALIGNED);
  PWM_SET_ALIGNED_TYPE(PWM, 5, PWM_EDGE_ALIGNED);  // PWM_CENTER_ALIGNED

  // Enable PWM channel 0 output
  PWM_EnableOutput(PWM, BIT1);
  PWM_EnableOutput(PWM, BIT4);
  PWM_EnableOutput(PWM, BIT5);

  // Start
  PWM_Start(PWM, BIT1);
  PWM_Start(PWM, BIT4);
  PWM_Start(PWM, BIT5);

  PWM_SET_CNR(PWM, 1, MOT_PWM_C_CYCLE);
  PWM_SET_CMR(PWM, 1, MOT_PWM_C_CYCLE / 2);

  PWM_SET_CNR(PWM, 4, MOT_PWM_B_CYCLE);
  PWM_SET_CMR(PWM, 4, MOT_PWM_B_CYCLE / 2);

  PWM_SET_CNR(PWM, 5, MOT_PWM_A_CYCLE);
  PWM_SET_CMR(PWM, 5, MOT_PWM_A_CYCLE / 2);
}

void bsp_mot_set(MOT_GN_E gn, uint8_t dir, uint8_t en) {
  if (gn == MOT_GN_A) {
    (dir) ? (MOT_DIR_A_PIN = 0) : (MOT_DIR_A_PIN = 1);
    (en) ? (MOT_EN_A_PIN = 0) : (MOT_EN_A_PIN = 1);
  }
  if (gn == MOT_GN_B) {
    (dir) ? (MOT_DIR_B_PIN = 0) : (MOT_DIR_B_PIN = 1);
    (en) ? (MOT_EN_B_PIN = 0) : (MOT_EN_B_PIN = 1);
  }
  if (gn == MOT_GN_C) {
    (dir) ? (MOT_DIR_C_PIN = 0) : (MOT_DIR_C_PIN = 1);
    (en) ? (MOT_EN_C_PIN = 0) : (MOT_EN_C_PIN = 1);
  }
}

void bsp_mot_set_step(MOT_GN_E gn, uint8_t step_value) {
  uint16_t pwm_cycle = 0;
  switch (gn) {
    case MOT_GN_A:
      /* code */
      pwm_cycle = MOT_PWM_A_MIN + ((MOT_PWM_A_MAX-MOT_PWM_A_MIN)/100*step_value);
      PWM_SET_CNR(PWM, 5, pwm_cycle);
      PWM_SET_CMR(PWM, 5, pwm_cycle / 2);
      break;
    case MOT_GN_B:
     pwm_cycle = MOT_PWM_B_MIN + ((MOT_PWM_B_MAX-MOT_PWM_B_MIN)/100*step_value);
      PWM_SET_CNR(PWM, 4, pwm_cycle);
      PWM_SET_CMR(PWM, 4, pwm_cycle / 2);
      break;
    case MOT_GN_C:
      /* code */
      pwm_cycle = MOT_PWM_C_MIN + ((MOT_PWM_C_MAX-MOT_PWM_C_MIN)/100*step_value);
      PWM_SET_CNR(PWM, 1, pwm_cycle);
      PWM_SET_CMR(PWM, 1, pwm_cycle / 2);
      break;

    default:
      break;
  }
}
void bsp_fan_set(uint8_t sta) {
  (sta) ? (FAN_PIN = 1) : (FAN_PIN = 0);
}